#include "zf_common_headfile.h"
/**************************************/
unsigned long Picture_Count=1;//圆环820  //十字

Image_DataTypedef Image_DataStruct;
/*图像处理数组*/
uint8_t Gray_Image[ROW][COL];


 ImageState stateQueue[QUEUE_SIZE]; // 用于存储最近5幅图像的状态
 ImageState currentState;//存储当前图像状态

unsigned char queueIndex = 0; // 队列的索引
void ImageQueue(void)
{
        // 将当前状态加入到队列中
        stateQueue[queueIndex] = currentState;
        // 更新队列索引
        queueIndex = (queueIndex + 1) % QUEUE_SIZE;

            unsigned char count = 0;
for (unsigned char i = 0; i < QUEUE_SIZE; ++i) 
{
    if (stateQueue[i] == IN_PROGRAM) count++;
    else count = 0; // 如果遇到不是 IN_PROGRAM 的状态，重置计数器
}
if(count==5){System_DataStruct.Element_Flag=RIGHT_CIRCLE_IN;printf("queue %d\n",System_DataStruct.CIRCLE_Flag);}

}

#define IMAGE_HANDLE 0 //0代表老代码 纯灰度  1代表新代码 大津法 提供更强的抗干扰 面对光的不均匀
void ImageProcess(void)
{
memcpy(Gray_Image,mt9v03x_image,MT9V03X_IMAGE_SIZE);
mt9v03x_finish_flag=0;

#if IMAGE_HANDLE==1
Image_DataStruct.Ostu_Threshold = otsuThreshold(Gray_Image[0],20);
#endif
get_reference_point(Gray_Image[0]);

#if IMAGE_HANDLE==0
longest_column(Gray_Image[0],LSC_STEP,COL-LSC_STEP);
#elif IMAGE_HANDLE==1
longest_column_new(Gray_Image[0],LSC_STEP,COL-LSC_STEP);
#endif

if(System_DataStruct.stop_flag!=-1)
{
    search_line(Gray_Image[0],ROW_Start,ROW-Image_DataStruct.lst_rows+LSR_STEP);
    iptsr_num = (Image_DataStruct.lst_rows-LSC_STEP)<0?0:(Image_DataStruct.lst_rows-LSC_STEP);
    iptsL_num = (Image_DataStruct.lst_rows-LSC_STEP)<0?0:(Image_DataStruct.lst_rows-LSC_STEP);
ExtractCenterline();
Island_Judge();
Island_Process();
if(Garage_find(Image_DataStruct.begin_x-20,Image_DataStruct.begin_x+20) == 1)
{ /*gpio_toggle_level(BUZZER_PIN);*/ Image_DataStruct.Camera_Error=0;}
}



// ImageQueue();//图像队列,连续图像记录

Image_DataStruct.Camera_Error = Get_Error();
printf("Camera_Error: %f\n",Image_DataStruct.Camera_Error);
}


#define SYNOVIUM 25 //滑膜区域
#define SYNOVIUM_BOUNDARY 79
//int synovium;
float Get_Error(void)
{
   float err=0;
   static float err_1;
   static int lock=0;
   uint8_t weight_count=0;

if(System_DataStruct.stop_flag !=-1)
{

    unsigned char vertex = ROW_Start-rpts_num; //顶点位置
    int synovium = (SYNOVIUM_BOUNDARY- vertex)-SYNOVIUM; //  富裕点数减去滑膜区域 正数不处理 负数滑膜下移
    if(synovium>=0) // 大于滑膜区域 滑膜边界点以下不参与计算
    {
        weight_count=0;
                for (uint8_t i=ROW_Start-rpts_num;i<=SYNOVIUM_BOUNDARY;i++){
                err+=(COL/2-rpts[i])*1.1;
                weight_count += 1;
                }
                err = err/weight_count;
    }
    else if(synovium<0 && synovium>=-SYNOVIUM) // -40 ~ 0 滑膜边界区域
    {
        weight_count=0;
        for (uint8_t i=ROW_Start-rpts_num;i<=SYNOVIUM_BOUNDARY-synovium;i++) // 79+40
        {
        err+=(COL/2-rpts[i])*2.2;
        weight_count += 1;
        }
        err = err/weight_count;
    }
    else // 这里说明 富裕点数<=-40 说明顶点位置
    {
        weight_count=0;
//        gpio_toggle_level(BUZZER_PIN);
        for (uint8_t i=ROW_Start;i>=ROW_Start-rpts_num;i--) // 79+40
        {
        err+=(COL/2-rpts[i])*1;
        weight_count += 1;
        }
        err = err/weight_count;
    }



/**************************误差特调专区***********************************/
    switch(System_DataStruct.Element_Flag)
    {

        case LEFT_CIRCLE_IN://左环岛 正偏差
            switch(System_DataStruct.CIRCLE_Flag)
            {
                case CIRCLE_PREV_ARC:
                    if(err<=0){err=40;}
                    break;
                case CIRCLE_WILL_IN:
                    if(err<=0){err=50;}
                break;
                case CIRCLE_HAVE_IN:
                    if(err<=50){err=50;}

                    break;
                case CIRCLE_TURN_D:
                    if(err<0)err=50; //
                    if(err>=60)err=60;
                    break;
                case CIRCLE_OUT_U:
                    if(err>=60)err=60;
                    break;
                    default :
                    break;
            }
            break;
        case RIGHT_CIRCLE_IN: //右环岛 负偏差
            switch(System_DataStruct.CIRCLE_Flag)
            {
                case CIRCLE_PREV_ARC:
                    if(err>=0){err=-50;}
                    break;
                case CIRCLE_WILL_IN:
                    if(err>=0)err=-50;
                    break;
                case CIRCLE_HAVE_IN:
                    if(err>=0)err=-50;
                    break;
                case CIRCLE_TURN_D:
                    if(err>0)err=-50; //右环岛 不允许出现 误差大于0的情况
                    break;
                case CIRCLE_OUT_U:
                    if(err<=-60)err=-60;
                    break;
                    default :
                    break;
            }
            break;
        case GARAGE_FIND:
                err=0;
                break;
        default :
            if(err*err_1<0)
            {
                if(fabs(err_1)>=50)
                {
                    err = err_1;
                }
//                    gpio_toggle_level(BUZZER_PIN);
            }
            break;
    }

}
/*********************出界处理程序**************************************/
if(System_DataStruct.stop_flag ==-1 && lock==0){
    switch(System_DataStruct.Element_Flag)
    {

        case LEFT_CIRCLE_IN:
            lock=1;// 左环岛 出界 锁定左转
            break;
        case RIGHT_CIRCLE_IN:
            lock=-1; //右环岛 出界 锁定右转
            break;
        default :
                if(err_1<0)lock=-1; //向右转
                if(err_1>0)lock=1; //向左转
            break;
    }

}

if(lock !=0 && Image_DataStruct.lst_rows>=30){lock=0;} //解除锁定状态

switch(lock) //
{
case 1: //向左转
    err = 75; //75

    break;
case -1: //err_1<0 向右拐
    err= -75; //-75
    break;
    default :
        break;
}


err_1 = err;
return err;
}


/*!
 *  @brief      大津法二值化0.8ms程序
 *  @date:   2018-10
 *  @since      v1.2
 *  *image ：图像地址
 *  width:  图像宽
 *  height：图像高
 *  @author     Z小旋
 */
unsigned char otsuThreshold(unsigned char *image,unsigned char row_num)
{
    #define GrayScale 256
    int pixelCount[GrayScale] = {0};//每个灰度值所占像素个数
    float pixelPro[GrayScale] = {0};//每个灰度值所占总像素比例
    int i,j;
    int Sumpix = COL * row_num/2;   //总像素点
    unsigned char threshold = 0;
    unsigned char* data = image;  //指向像素数据的指针


    //统计灰度级中每个像素在整幅图像中的个数
    for (i = ROW_Start; i >=ROW_Start-row_num; i--)
    {
        for (j = 0; j < COL; j+=2)
        {
            pixelCount[(int)data[i * COL + j]]++;  //将像素值作为计数数组的下标
        }
    }
    float u = 0;
    for (i = 0; i < GrayScale; i++)
    {
        pixelPro[i] = (float)pixelCount[i] / Sumpix;   //计算每个像素在整幅图像中的比例
        u += i * pixelPro[i];  //总平均灰度
    }


    float maxVariance=0.0;  //最大类间方差
    float w0 = 0, avgValue  = 0;  //w0 前景比例 ，avgValue 前景平均灰度
    for(int i = 0; i < 256; i++)     //每一次循环都是一次完整类间方差计算 (两个for叠加为1个)
    {
        w0 += pixelPro[i];  //假设当前灰度i为阈值, 0~i 灰度像素所占整幅图像的比例即前景比例
        avgValue  += i * pixelPro[i];

        float variance = pow((avgValue/w0 - u), 2) * w0 /(1 - w0);    //类间方差
        if(variance > maxVariance)
        {
            maxVariance = variance;
            threshold = i>=255?255:i;
        }
    }

    return threshold;

}

